#include "run.h"
#include "bsp_can.h"
#include "pid.h"
#include "uart.h"
#include "E18SENSOR.h"

#define ABS(x)	( (x>0) ? (x) : (-x) )

uint32_t time=0;
uint32_t dtime=0;

int lock_num=0;

void run_pos(int pos1,int pos2,int pos3,int pos4,int pos5,int pos6)
{
		para_s[0].target=pos1;
    para_s[1].target=pos2;
	  para_s[2].target=pos3; 
    para_s[3].target=pos4;
	  para_s[4].target=pos5;
		para_s[5].target=pos6;
while(1)
{
	pid_calculate_s(&para_s[0],&para_v[0],motor[0].total_angle,motor[0].real_current);
	pid_calculate_s(&para_s[1],&para_v[1],motor[1].total_angle,motor[1].real_current);
	pid_calculate_s(&para_s[2],&para_v[2],motor[2].total_angle,motor[2].real_current);
	pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
	pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
  pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,para_v[0].output,para_v[1].output,para_v[2].output,para_v[5].output);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,para_v[3].output,para_v[4].output,0,0);
	HAL_Delay(1);

//printf("%f,%d,\r\n",motor[4].total_angle/8191.0,motor[3].real_current);
	if(ABS(para_s[0].error)<2800 && ABS(motor[0].real_current)<700 && ABS(para_s[1].error)<2800 && ABS(motor[1].real_current)<700 && ABS(para_s[2].error)<3500 && ABS(motor[2].real_current)<300 && ABS(para_s[3].error)<2500 && ABS(motor[3].real_current)==0 && ABS(para_s[4].error)<4000 && ABS(motor[4].real_current)<1500)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
}
}


void run_pos_turnning3(int pos1,int pos2,int pos3,int pos4,int pos5,int pos6)
{
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;
	  para_s[2].target=pos3; 
//    para_s[3].target=motor[3].total_angle;
	  para_s[4].target=motor[4].total_angle;
		para_s[5].target=-30*8191;
while(1)
{
//	pid_calculate_s(&para_s[0],&para_v[0],motor[0].total_angle,motor[0].real_current);
//	pid_calculate_s(&para_s[1],&para_v[1],motor[1].total_angle,motor[1].real_current);
	pid_calculate_s(&para_s[2],&para_v[2],motor[2].total_angle,motor[2].real_current);
//	pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
	pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
  pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,para_v[2].output,para_v[5].output);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,para_v[4].output,0,0);
	HAL_Delay(1);

//printf("%f,%d,\r\n",para_s[5].error,motor[5].real_current);
	if( ABS(para_s[2].error)<5000 && ABS(motor[2].real_current)<100)
	{
		set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1000);
		HAL_Delay(1);
		set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
		HAL_Delay(1);
		break;
	}
}
}


void run_pos_turnning35(int pos1,int pos2,int pos3,int pos4,int pos5,int pos6)
{
	int num8=0;
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;
	  para_s[2].target=pos3; 
//    para_s[3].target=motor[3].total_angle;
	  para_s[4].target=pos5;
		para_s[5].target=-30*8191;
while(1)
{
	num8++;
//	pid_calculate_s(&para_s[0],&para_v[0],motor[0].total_angle,motor[0].real_current);
//	pid_calculate_s(&para_s[1],&para_v[1],motor[1].total_angle,motor[1].real_current);
	pid_calculate_s(&para_s[2],&para_v[2],motor[2].total_angle,motor[2].real_current);
//	pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
	pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
  pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,para_v[2].output,para_v[5].output);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,para_v[4].output,0,0);
	HAL_Delay(1);

//printf("%f,%d,\r\n",para_s[5].error,motor[5].real_current);
	if( ABS(para_s[2].error)<5000 && ABS(motor[2].real_current)<100 &&  ABS(para_s[4].error)<6000 && ABS(motor[4].real_current)<100)
	{
		set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1000);
		HAL_Delay(1);
		set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
		HAL_Delay(1);
		break;
	}
	if(num8>2000)
	{
		set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1000);
		HAL_Delay(1);
		set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
		HAL_Delay(1);
		break;
	}
}


}


void run_pos_turnning3_open(int pos1,int pos2,int pos3,int pos4,int pos5,int pos6)
{
	int num5=0;
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;
	  para_s[2].target=pos3; 
//    para_s[3].target=motor[3].total_angle;
	  para_s[4].target=motor[4].total_angle;
		para_s[5].target=20*8191;
while(1)
{
//	pid_calculate_s(&para_s[0],&para_v[0],motor[0].total_angle,motor[0].real_current);
//	pid_calculate_s(&para_s[1],&para_v[1],motor[1].total_angle,motor[1].real_current);
	num5++;
	pid_calculate_s(&para_s[2],&para_v[2],motor[2].total_angle,motor[2].real_current);
	pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
	pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
  pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,para_v[2].output,para_v[5].output);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,para_v[3].output,para_v[4].output,0,0);
	HAL_Delay(1);

//printf("%f,%d,\r\n",motor[4].total_angle/8191.0,motor[3].real_current);
	if( ABS(para_s[2].error)<4800 && ABS(motor[2].real_current)<100)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
	
	if(num5>2000)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
	
}
}

void run_pos_turnning4(int pos1,int pos2,int pos3,int pos4,int pos5,int pos6)
{
//		time=HAL_GetTick();
//	  dtime=0; 
	int num1=0;
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;
    para_s[3].target=pos4;
	  para_s[4].target=pos5;
		para_s[5].target=-30*8191;
while(1)
{
	num1++;
	pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
	pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
  pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,para_v[5].output);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,para_v[3].output,para_v[4].output,0,0);
	HAL_Delay(1);

//printf("%d,\r\n",motor[3].total_angle);
printf("%f,\r\n",para_s[3].error);
	if( ABS(para_s[3].error)<2500 && ABS(motor[3].real_current)==0)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1000);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
	if(num1>2000)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1000);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	
	}

}
}


void run_pos_turnning4_open(int pos1,int pos2,int pos3,int pos4,int pos5,int pos6)
{
	int num3=0;
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;
	
    para_s[5].target=30*8191;

		para_s[3].target=pos4;
	  para_s[4].target=pos5;
while(1)
{
	num3++;
	pid_calculate_s(&para_s[2],&para_v[2],motor[2].total_angle,motor[2].real_current);
	pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
	pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
  pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,para_v[5].output);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,para_v[3].output,para_v[4].output,0,0);
	HAL_Delay(1);

//printf("%f,\r\n",para_s[3].error);
	if( ABS(para_s[3].error)<3700 && ABS(motor[3].real_current)==0)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
	if(num3>1200)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
}
}
void run_pos_down5(int pos1,int pos2,int pos3,int pos4,int pos5,int pos6)
{
	int num2=0;
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;

	  para_s[4].target=pos5;
		para_s[5].target=-30*8191;
while(1)
{
	num2++;
	pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
	pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
  pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,para_v[5].output);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,para_v[4].output,0,0);
	HAL_Delay(1);

   printf("%f,\r\n",para_s[4].error);
//	printf("%d\r\n",motor[4].real_current);
	if(ABS(para_s[4].error)<6000 && ABS(motor[4].real_current)<200)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1000);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
	if(num2>2000)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1000);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
}
}


void run_pos_down_test(int pos1,int pos2,int pos3,int pos4,int pos5,int pos6)
{
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;
	
//    para_s[3].target=motor[3].total_angle;
	  para_s[4].target=pos5;
		para_s[5].target=-30*8191;
while(1)
{
	pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
	pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
  pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,para_v[5].output);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,para_v[4].output,0,0);
	HAL_Delay(1);

   printf("%f\r\n",para_s[4].error);
	printf("%d\r\n",motor[4].real_current);
//	if(ABS(para_s[4].error)<5000 && ABS(motor[4].real_current)<1500)
//	{
//	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1000);
//	HAL_Delay(1);
//	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
//	HAL_Delay(1);
//	break;
//	}
}
}
void run_pos_down5_open(int pos1,int pos2,int pos3,int pos4,int pos5,int pos6)
{
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;
	
    para_s[5].target=30*8191;
	  para_s[4].target=pos5;
	int num2=0;
	
while(1)
{
	num2++;
	pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
	pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
  pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,para_v[5].output);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,para_v[3].output,para_v[4].output,0,0);
	HAL_Delay(1);


	if(ABS(para_s[4].error)<3000 && ABS(motor[4].real_current)<100)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
	if(num2>3000)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
}
}

void run_pos_down5_open_loop(int pos1,int pos2,int pos3,int pos4,int pos5,int pos6)
{
 //   para_s[3].target=motor[3].total_angle;
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;
	
    para_s[5].target=30*8191;
	  para_s[4].target=pos5;
while(1)
{
	pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
	pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
  pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,para_v[5].output);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,para_v[4].output,0,0);
	HAL_Delay(1);
	printf("%f,\r\n",para_s[4].error);
}
}

void run_pos_forward(int pos1,int pos2,int pos3,int pos4,int pos5,int pos6)
{
	int num7=0;
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;

		para_s[0].target=pos1;
    para_s[1].target=pos2;
//	  para_s[2].target=motor[2].total_angle; 
//    para_s[3].target=motor[3].total_angle; 
//	  para_s[4].target=motor[4].total_angle; 
		para_s[5].target=-30*8191;
while(1)
{
	num7++;
	pid_calculate_s(&para_s[0],&para_v[0],motor[0].total_angle,motor[0].real_current);
	pid_calculate_s(&para_s[1],&para_v[1],motor[1].total_angle,motor[1].real_current);
	pid_calculate_s(&para_s[2],&para_v[2],motor[2].total_angle,motor[2].real_current);
	pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
	pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
  pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,para_v[0].output,para_v[1].output,para_v[2].output,para_v[5].output);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,para_v[3].output,para_v[4].output,0,0);
	HAL_Delay(1);

	printf("%d,%d,\r\n",motor[0].total_angle,motor[0].real_current);
	if(ABS(para_s[0].error)<30000 && ABS(motor[0].real_current)<100 && ABS(para_s[1].error)<30000 && ABS(motor[1].real_current)<100)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
	
	if(num7>1000)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
	
}
}

void run_pos_back(int pos1,int pos2,int pos3,int pos4,int pos5,int pos6)
{
 int num8=0;
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;

		para_s[0].target=pos1;
    para_s[1].target=pos2;
//	  para_s[2].target=motor[2].total_angle; 
//    para_s[3].target=motor[3].total_angle; 
	  para_s[4].target=pos5; 
		para_s[5].target=-30*8191;
while(1)
{
	num8++;
	pid_calculate_s(&para_s[0],&para_v[0],motor[0].total_angle,motor[0].real_current);
	pid_calculate_s(&para_s[1],&para_v[1],motor[1].total_angle,motor[1].real_current);
	pid_calculate_s(&para_s[2],&para_v[2],motor[2].total_angle,motor[2].real_current);
	pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
	pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
  pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,para_v[0].output,para_v[1].output,para_v[2].output,para_v[5].output);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,para_v[3].output,para_v[4].output,0,0);
	HAL_Delay(1);

//printf("%f,%d,\r\n",motor[4].total_angle/8191.0,motor[3].real_current);
	if(ABS(para_s[0].error)<30000 && ABS(motor[0].real_current)<500 && ABS(para_s[1].error)<30000 && ABS(motor[1].real_current)<500 &&ABS(para_s[4].error)<6000 && ABS(motor[4].real_current)<100)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
	
	if(num8>2500)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}
	
}
}
void run_pos_open()
{
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;
		time=HAL_GetTick();
	  dtime=0; 
//	  para_s[2].target=motor[2].total_angle; 
//    para_s[3].target=motor[3].total_angle; 
	  para_s[4].target=motor[4].total_angle; 
		para_s[5].target=30*8191;
while(1)
{
	printf("%f,%d,\r\n",para_s[5].error,motor[5].real_current);
	dtime=HAL_GetTick()-time;
	pid_calculate_s(&para_s[0],&para_v[0],motor[0].total_angle,motor[0].real_current);
	pid_calculate_s(&para_s[1],&para_v[1],motor[1].total_angle,motor[1].real_current);
	pid_calculate_s(&para_s[2],&para_v[2],motor[2].total_angle,motor[2].real_current);
	pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
	pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
  pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,para_v[5].output);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,para_v[4].output,0,0);
	HAL_Delay(1);

	if(para_s[5].error<73000 && motor[5].real_current<50)
	{
		set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
		HAL_Delay(1);
		set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
		HAL_Delay(1);
		break;
	}
	if(dtime>2500)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	HAL_Delay(1);
	break;
	}

}
}

void pos_adjust4(int pos)
{
		int num4=0;
	  para_s[3].target=motor[3].total_angle+pos;
		para_s[5].target=-50*8191;
	while(1)
	{
		num4++;
//		pid_calculate_s(&para_s[0],&para_v[0],motor[0].total_angle,motor[0].real_current);
//	  pid_calculate_s(&para_s[1],&para_v[1],motor[1].total_angle,motor[1].real_current);
		pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
		pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
		
		set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,para_v[5].output);
	  HAL_Delay(1);
		set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,para_v[3].output,0,0,0);
	  HAL_Delay(1);

		
		printf("%f,%d,\r\n",para_s[3].error,motor[3].real_current);
		if(ABS(para_s[3].error)<6500&& motor[3].real_current==0)
		{
			set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1000);
			HAL_Delay(1);
			set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
			HAL_Delay(1);
			break;
		}		
		
		if(num4>1200)
		{
			set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1000);
			HAL_Delay(1);
			set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
			HAL_Delay(1);
			break;
		}
	}
	
}

void pos_adjust44(int pos)
{
		int num6=0;
	  para_s[3].target=motor[3].total_angle+pos;
		para_s[5].target=-50*8191;
	while(1)
	{
		num6++;
//		pid_calculate_s(&para_s[0],&para_v[0],motor[0].total_angle,motor[0].real_current);
//	  pid_calculate_s(&para_s[1],&para_v[1],motor[1].total_angle,motor[1].real_current);
		pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
		pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
		
		set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,para_v[5].output);
	  HAL_Delay(1);
		set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,para_v[3].output,0,0,0);
	  HAL_Delay(1);
		printf("%f,%d,\r\n",para_s[3].error,motor[3].real_current);
		if(ABS(para_s[3].error)<2000 && motor[3].real_current==0)
		{
			set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1000);
			HAL_Delay(1);
			set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
			HAL_Delay(1);
			break;
		}		
		
		
		if(num6>1200)
		{
			set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1000);
			HAL_Delay(1);
			set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
			HAL_Delay(1);
			break;
		}
	}
	
}

void speed_adjust22(int speed)
{
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;
	  para_v[2].target=speed;
//		para_s[3].target=motor[3].total_angle;
//		para_s[4].target=motor[4].total_angle;
		para_s[5].target=-35*8191;
	while(1)
	{
		pid_calculate_v(&para_v[2],motor[2].real_current);
		
//		pid_calculate_s(&para_s[0],&para_v[0],motor[0].total_angle,motor[0].real_current);
//	  pid_calculate_s(&para_s[1],&para_v[1],motor[1].total_angle,motor[1].real_current);
//		pid_calculate_s(&para_s[3],&para_v[3],motor[3].total_angle,motor[3].real_current);
//		pid_calculate_s(&para_s[4],&para_v[4],motor[4].total_angle,motor[4].real_current);
		pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
		
		set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,para_v[2].output,para_v[5].output);
	  HAL_Delay(1);
		set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
	  HAL_Delay(1);
		
		leftright_sensor();
		if(flag_left==1 && flag_right==1)
		{
			set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1500);
			HAL_Delay(1);
			set_moto_current1(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,0);
			HAL_Delay(1);
			break;
		}
	}
	
}


void position_check22()
{
	while(1)
	{
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1000);
	HAL_Delay(1);
	leftright_sensor();
	if(flag_left==1 && flag_right==0)
		{
			pid_speed_slow_back();
			speed_adjust22(-1000);
		}
	 if(flag_left==0 && flag_right==1)
		{
			pid_speed_slow();
			speed_adjust22(600);
		}
	 if(flag_left==0 && flag_right==0)
		{
			reg=1;
			break;
		}
	HAL_Delay(100);
	leftright_sensor();
	if(flag_left==1 && flag_right==1)
		{
					HAL_Delay(100);
					leftright_sensor();
			if(flag_left==1 && flag_right==1)
			{
					break;
			}
		}
	}
}
// void pid_antiturning_slow()
// {
//	int m=0;
//			for(m=0;m<5;m++)
//	{
//		pid_Init(0.13,0.5,1,1,800,9000,5000,&para_s[m]);
//		pid_Init(1,0.1,1,1,700,3000,8000,&para_v[m]);
//	}
// }
// void pid_turning_slow()
// {
//	int m=0;
//			for(m=0;m<5;m++)
//	{
//		pid_Init(0.16,0.6,5.48,1,1000,9000,4500,&para_s[m]);
//		pid_Init(1,0.3,1,1,900,3000,8000,&para_v[m]);
//	}
// }




 void pid_forward1()
 {
		pid_Init(0.08,0.06,2.5,10,500,9000,6000,&para_s[0]);
		pid_Init(1,0,0.6,10,700,7000,8000,&para_v[0]);
	 
		pid_Init(0.1,0.066,1.3,10,500,9000,7000,&para_s[1]);
		pid_Init(1,0,0.6,10,700,8000,8000,&para_v[1]);
	 

		pid_Init(0.055,0.8,6,1,800,9000,4000,&para_s[2]);
		pid_Init(1,0.3,1,1,900,3000,8000,&para_v[2]);
//	 
		pid_Init(0.05,0.5,4.5,1,600,9000,3000,&para_s[3]);
		pid_Init(0.6,0,0,1,900,2500,8000,&para_v[3]);
	 
	 	pid_Init(0.15,0.1,2.5,10,1000,9500,6000,&para_s[4]);
		pid_Init(1,0,0.6,10,700,8000,8000,&para_v[4]);
 }
 
  void pid_forward()
 {
		pid_Init(0.08,0.06,2.5,10,800,9000,7000,&para_s[1]);
		pid_Init(1,0,0.6,10,700,8500,8000,&para_v[1]);
	 
		pid_Init(0.05,0.066,1.3,10,500,9000,6000,&para_s[0]);
		pid_Init(1,0,0.6,10,700,7000,8000,&para_v[0]);

		pid_Init(0.07,0.8,7,1,800,9000,3600,&para_s[2]);
		pid_Init(1,0,0.6,1,900,3300,8000,&para_v[2]);
	 
		pid_Init(0.07,0.01,3,1.5,500,9000,4000,&para_s[3]);
		pid_Init(0.9,0,1,1,900,4000,8000,&para_v[3]);

	 	pid_Init(0.15,0.1,2.5,10,700,9500,3000,&para_s[4]);
		pid_Init(1,0,0.6,10,700,2000,8000,&para_v[4]);
	 
	 	pid_Init(1.5,0.9,4,1,900,9000,7000,&para_s[5]);
		pid_Init(1.5,0,0,1,900,4500,8000,&para_v[5]);
 }
 
   void pid_forward0()
 {
		pid_Init(0.08,0.06,2.5,10,800,9000,7000,&para_s[1]);
		pid_Init(1,0,0.6,10,700,8500,8000,&para_v[1]);
	 
		pid_Init(0.05,0.066,1.3,10,500,9000,6000,&para_s[0]);
		pid_Init(1,0,0.6,10,700,7000,8000,&para_v[0]);

		pid_Init(0.1,0.8,5,1,800,9000,3500,&para_s[2]);
		pid_Init(1,0,0.6,1,900,3000,8000,&para_v[2]);
	 
		pid_Init(0.07,0.01,4,1.5,500,9000,4000,&para_s[3]);
		pid_Init(1,0,1,1,900,3500,8000,&para_v[3]);

	 	pid_Init(0.2,0.1,1.5,10,900,9500,5000,&para_s[4]);
		pid_Init(1,0,0.6,10,700,4500,8000,&para_v[4]);
	 
	 	pid_Init(1.5,0.9,4,1,900,9000,7000,&para_s[5]);
		pid_Init(1.5,0,0,1,900,4500,8000,&para_v[5]);
 }
 
 
 
   void pid_adjust44()
 {
	 
		pid_Init(0.3,0.1,9,1,1500,9000,2000,&para_s[3]);
		pid_Init(0.6,0,0,1,900,1500,8000,&para_v[3]);

	 
	 	pid_Init(1.5,0.9,4,1,900,9000,7000,&para_s[5]);
		pid_Init(1.5,0,0,1,900,4500,8000,&para_v[5]);
 }
    void pid_adjust444()
 {
	 
		pid_Init(0.3,0.1,9,1,700,9000,2000,&para_s[3]);
		pid_Init(0.6,0,0,1,900,1500,8000,&para_v[3]);

	 
	 	pid_Init(1.5,0.9,4,1,900,9000,7000,&para_s[5]);
		pid_Init(1.5,0,0,1,900,4500,8000,&para_v[5]);
 }
 
   void pid_forward_open()
 {
	 	pid_Init(0.1,0.066,1.3,10,500,9000,7000,&para_s[0]);
		pid_Init(1,0,0.6,10,700,8000,8000,&para_v[0]);
	 
		pid_Init(0.08,0.06,2.5,10,500,9000,6000,&para_s[1]);
		pid_Init(1,0,0.6,10,700,7000,8000,&para_v[1]);

		pid_Init(0.08,0.4,7,1,800,9000,3500,&para_s[2]);
		pid_Init(1,0,0.6,1,900,3400,8000,&para_v[2]);
	 
		pid_Init(0.25,0,0.8,1.5,500,9000,6000,&para_s[3]);
		pid_Init(0.6,0,1,1,900,5000,8000,&para_v[3]);

	 
	 	pid_Init(0.15,0.1,2.5,10,700,9500,5000,&para_s[4]);
		pid_Init(1,0,0.6,10,700,4500,8000,&para_v[4]);

//		pid_Init(0.04,0.01,7,1,600,9000,3000,&para_s[5]);
//		pid_Init(0.6,0,0,1,900,3000,8000,&para_v[5]);

	 	pid_Init(0.15,0.05,5,1,600,9000,6000,&para_s[5]);
		pid_Init(0.6,0,0,1,900,6500,8000,&para_v[5]);
 }
 
 void pid_turnning4_open()
 {
		pid_Init(0.1,0.066,1.3,10,500,9000,7000,&para_s[0]);
		pid_Init(1,0,0.6,10,700,8000,8000,&para_v[0]);
	 
		pid_Init(0.08,0.06,2.5,10,500,9000,6000,&para_s[1]);
		pid_Init(1,0,0.6,10,700,7000,8000,&para_v[1]);

		pid_Init(0.07,0.8,7,1,800,9000,3500,&para_s[2]);
		pid_Init(1,0,0.6,1,900,3000,8000,&para_v[2]);
	 
		pid_Init(0.07,0.01,3,1.5,500,9000,5000,&para_s[3]);
		pid_Init(1,0,1,1,900,4000,8000,&para_v[3]);
	 
	 	pid_Init(0.15,0.1,2.5,10,700,9500,3000,&para_s[4]);
		pid_Init(1,0,0.6,10,700,2000,8000,&para_v[4]);
	 
	 	pid_Init(1.5,0.9,4,1,900,9000,6000,&para_s[5]);
		pid_Init(1.5,0,0,1,900,3500,8000,&para_v[5]);
 
 
 }
   void pid_adjust4()
 {

		pid_Init(0.4,0,0.8,1.5,500,9000,6000,&para_s[3]);
		pid_Init(0.5,0,1,1,900,5500,8000,&para_v[3]);
	 
	 	pid_Init(1.5,0.9,4,1,900,9000,7000,&para_s[5]);
		pid_Init(1.5,0,0,1,900,4500,8000,&para_v[5]);
 }
 
 
  void pid_back()
 {
		pid_Init(0.08,0.06,2.5,10,800,9000,7000,&para_s[0]);
		pid_Init(1,0,0.6,10,700,8500,8000,&para_v[0]);
	 
		pid_Init(0.05,0.066,1.3,10,500,9000,6000,&para_s[1]);
		pid_Init(1,0,0.6,10,700,7000,8000,&para_v[1]);
	 

		pid_Init(0.06,0.8,5,1,1000,9000,4000,&para_s[2]);
		pid_Init(1,0.3,1,1,900,3000,8000,&para_v[2]);
	 
		pid_Init(0.03,0.9,7,1,900,9000,2500,&para_s[3]);
		pid_Init(0.6,0,0,1,900,2000,8000,&para_v[3]);

	 	pid_Init(0.2,0.1,1.5,10,900,9500,5000,&para_s[4]);
		pid_Init(1,0,0.6,10,700,4500,8000,&para_v[4]);
	 
	 	pid_Init(1.5,0.9,4,1,900,9000,7000,&para_s[5]);
		pid_Init(1.5,0,0,1,900,4500,8000,&para_v[5]);
	 
 }
 
  void pid_speed_slow()
 {	 
		pid_Init(0.5,0,1,1,900,2000,6000,&para_v[2]);
	 	pid_Init(1.5,0.9,4,1,900,9000,7000,&para_s[5]);
		pid_Init(1.5,0,0,1,900,4500,8000,&para_v[5]);
 }
 
   void pid_speed_slow_back()
 {	 
		pid_Init(1.5,0,1,1,900,2800,6000,&para_v[2]);
	 	pid_Init(1.5,0.9,4,1,900,9000,7000,&para_s[5]);
		pid_Init(1.5,0,0,1,900,4500,8000,&para_v[5]);
 }
 
 
 
void lock_catch(int n,int* cola)
{	
	reg=0;
	int num=0;
	motor[5].total_angle=0;
	motor[5].round_cnt=0;
	motor[5].msg_cnt=0;
	motor[5].angle=0;
	motor[5].offset_angle=0;
	motor[5].given_current=0;
	para_s[5].target=-30*8191;
	while(1)
	{
	num++;
	pid_forward();
	pid_calculate_s(&para_s[5],&para_v[5],motor[5].total_angle,motor[5].real_current);
	set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,para_v[5].output);
	HAL_Delay(1);
	if(num==30)
	{
		position_check22();
		if(reg==1)
		{
			num=800;
		}
		else if(reg==0)
		{
			cola[6-n]=0;
		}
	}
	if(num==100 && reg==0)
	{
		lock_num++;
		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_1,GPIO_PIN_SET);
	}
	if(num==700 && reg==0)
	{
		HAL_GPIO_WritePin(GPIOB,GPIO_PIN_1,GPIO_PIN_RESET);
	}
	if(num==1250 && lock_num==3)
	{
		num=1400;
	}
	if(num==1300 && n<6)
	{
		pid_forward();
		run_pos_turnning3(0,0,1.85*8.9*8191+n*8.95*8191,0,0,0);
	}
	if(num==1500)
	{
		break;
	}
}
}
 
void cola_catch(int num,int temp)
{
	pid_forward();
	int num1=0;
	while(1)
	{
		num1++;
	  set_moto_current(&hcan,&TxHeader,TxData,&TxMailBox,0,0,0,-1500);
	  HAL_Delay(1);
		if(num1==10)
	{
		pid_forward();	
		switch(num)
			{
			case 0:
			{
					 run_pos_turnning4(0,0,0,-9*8191,-50*8191,0);
				   break;
			}
				case 1:
					{
						run_pos_turnning4(0,0,0,-9*8191-8.95*8191,-50*8191,0);
						break;
					}
				case 2:
					{
						run_pos_turnning4(0,0,0,-9*8191-8.95*8191*2,-50*8191,0);
						break;
					}			
				case 3:
					{
						run_pos_turnning4(0,0,0,-9*8191-8.95*8191*2-8.65*8191,-50*8191,0);
						break;
					}		
				case 4:
					{
						run_pos_turnning4(0,0,0,-9*8191-8.95*8191*2-8.65*8191-8.78*8191,-50*8191,0);
						break;
					}
				case 5:
					{
						run_pos_turnning4(0,0,0,-9*8191-8.95*8191*2-8.65*8191-8.78*8191-8.8*8191,-50*8191,0);
						break;
					}
			}
}
		if(num1==400)
		{
			pid_forward();	
			run_pos_down5(0,0,0,0,0,0);
		}
		if(num1==650 && temp==1)
		{
			pid_adjust44();
			pos_adjust4(1*8191);
		}
		if(num1==650 && temp==2)
		{
			 pid_adjust444();
			 pos_adjust44(-1.7*8191);
		}
		if(num1==1200 &&temp==1)
		{
			HAL_GPIO_WritePin(GPIOA,GPIO_PIN_1,GPIO_PIN_SET);
		}
		if(num1==1200 &&temp==2)
		{
			HAL_GPIO_WritePin(GPIOB,GPIO_PIN_2,GPIO_PIN_SET);
		}
		if(num1==1200 && temp==3)
		{
			HAL_GPIO_WritePin(GPIOB,GPIO_PIN_0,GPIO_PIN_SET);		
		}
		if(num1==2000)
		{
				pid_forward0();
		  	run_pos_down5(0,0,0,0,-50*8191,0);
		}
		if(num1==2100)
		{
			break;
		}

	}
	
}
 
 
 
 
 
 
 
 
 
 
 
 
 